import os, math
import cv2 as cv
import numpy as np
 
class PathPlan:
    def __init__(self):
        self.resize_para = 1
        self.img = None
        # self.img_path = 'md.png'
        self.img_path = '00731_1.png'
        self.xy_data = {}
        # self.xy_data_path = 'real_model_data.txt'
        self.xy_data_path = '00731.txt'

        # 读取图像
        self.img = cv.imread(self.img_path) 
        #self.img = cv.imread('/home/tan/Desktop/Arm/real_model.png')
        try:
            img_h, img_w = self.img.shape[:2]
        except AttributeError:
            print ("Error: could not load image")
            os._exit(0)

        self.resize_img(img_w, img_h)

    # 缩放图像：最小边固定600，另外一边按比例缩放
    def resize_img(self, img_w, img_h):
        if img_h > img_w:
            self.resize_para = 600 / img_w
            img_h = img_h * self.resize_para
            img_w = 600
        else:
            self.resize_para = 600 / img_h
            img_w = img_w * self.resize_para
            img_h = 600

        self.img = cv.resize(self.img, (int(img_w), int(img_h)), interpolation=cv.INTER_AREA)

    # 读取各部位坐标数据
    def read_xy_data(self):
        with open(self.xy_data_path, 'r') as file:
            # readlines()：读取文件全部内容，以列表形式返回结果
            data = file.readlines()
            print("----------列表形式----------")
            print(data)

            for item in data:
                item = item.strip('\n')
                tmp = item.split()
                self.xy_data[tmp[0]] = tmp[1:]

    # 颈部
    def draw_neck(self, LT_RB_xy):
        xy = self.count_rect_center(LT_RB_xy)
        cv.circle(self.img, xy, 3, (0, 0, 255), -1)

    # 肩
    def draw_shoulder(self, part_type, LT_RB_xy):
        #self.resize_para = 1 # 测试用，后面有真实数据要删除
        # 已知左上和右下两顶点坐标，求右上及左下
        x0 = int(round(float(LT_RB_xy[1]) * self.resize_para))
        y0 = int(round(float(LT_RB_xy[0]) * self.resize_para))
        x1 = int(round(float(LT_RB_xy[3]) * self.resize_para))
        y1 = int(round(float(LT_RB_xy[2]) * self.resize_para))

        left_top = (x0, y0) # 左上
        right_bottom = (x1, y1) # 右下
        right_top = (x1, y0) # 右上
        left_bottom = (x0, y1) # 左下

        ellipse_w = x1 - x0
        ellipse_h = y1 - y0

        if part_type == 'Lshoulder':
            start = left_top
        elif part_type == 'Rshoulder':
            start = right_top

        points = cv.ellipse2Poly(start, (ellipse_w, ellipse_h), 0, 0, 360, 8) # 生成曲线的坐标数据
        for i in range(len(points) - 1):
            if points[i][0] < left_top[0] or points[i][0] > right_top[0]:
                continue
            if points[i][1] < left_top[1] or points[i][1] > right_bottom[1]:
                continue

            #cv.line(self.img, (points[i][0], points[i][1]), (points[i + 1][0], points[i + 1][1]),(255, 0, 0), 1, cv.LINE_4, 0)
            cv.circle(self.img, tuple(points[i]), 3, (0, 0, 255), -1)  # 用红色圆圈标记

    # 背
    def draw_back(self, part_type, LT_RB_xy):
         # 已知左上和右下两顶点坐标，求右上及左下
        x0 = int(round(float(LT_RB_xy[1]) * self.resize_para))
        y0 = int(round(float(LT_RB_xy[0]) * self.resize_para))
        x1 = int(round(float(LT_RB_xy[3]) * self.resize_para))
        y1 = int(round(float(LT_RB_xy[2]) * self.resize_para))

        height = y1 - y0
        width = x1 - x0

        # 设置正弦波参数 y=A×sin(2πft+φ)
        """ y 表示波形在y轴方向上的值，即正弦波的振幅（波峰到波谷的距离的一半）。
        A 表示振幅，即波峰到波谷的距离的一半。
        f 表示频率（单位为Hz），表示波形在单位时间内震动的次数。
        t 表示时间。
        φ 表示相位角（单位为弧度或度数），表示波形的起始相位。 """
        A = width / 2 # 振幅
        B = height / 4  # 波的中心
        C = 10  # 波的波长
        D = 100  # 波的频率
        # 生成正弦波形的数据
        tmp = []
        for y in range(y0, y1, 8):
            x = int(A * np.sin(2 * np.pi * y / D + B) + (x0 + width / 2))
            #cv.line(self.img, (x, width-1), (y, x), (255, 255, 255), 1)
            cv.circle(self.img, (x, y), 3, (0, 0, 255), -1)

            tmp.append([x,y])
            print(x, y)
        print(len(tmp))

    # 求矩形中心坐标 (已知左上和右下两顶点坐标)
    def count_rect_center(self, lt_rb_xy):
        # self.resize_para = 1 # 测试用，后面有真实数据要删除
        x0 = int(round(float(lt_rb_xy[1]) * self.resize_para))
        y0 = int(round(float(lt_rb_xy[0]) * self.resize_para))
        x1 = int(round(float(lt_rb_xy[3]) * self.resize_para))
        y1 = int(round(float(lt_rb_xy[2]) * self.resize_para))

        x = x0 + ((x1 - x0) / 2)
        y = y0 + ((y1 - y0) / 2)
        center = (int(round(x)), int(round(y)))
        return center

    # 已知直径的两端的坐标，根据给出的方程(x-a)²+(y-b)²=r²，通过解二次方程来求得y
    def solve_y_for_circle(self, point1, point2, x):
        # 计算圆的中心点和半径
        a, b = ((point1[0] + point2[0]) // 2, (point1[1] + point2[1]) // 2)
        r = int(np.sqrt((point1[0] - point2[0]) ** 2 + (point1[1] - point2[1]) ** 2) / 2)

        # 判断x是否在圆的范围内
        if (x - a) ** 2 > r ** 2:
            return None  # x不在圆内，无解

        # 计算y的平方
        y_sqr = r ** 2 - (x - a) ** 2

        # 求解y的正负根
        y_sqr_sqrt = math.sqrt(y_sqr)
        y1 = b + y_sqr_sqrt
        y2 = b - y_sqr_sqrt

        return y1, y2  # 返回y的两个解

    # 臀部，尾椎 （以拆线连接）
    def draw_hip_x(self, data):
        # self.resize_para = 1 # 测试用，后面有真实数据要删除
        # 左臀中心坐标
        lhip_center = self.count_rect_center(data['Lhip'])
        cv.circle(self.img, lhip_center, 3, (0, 0, 255), -1)

        # 右臀中心坐标
        rhip_center = self.count_rect_center(data['Rhip'])
        cv.circle(self.img, rhip_center, 3, (0, 0, 255), -1)

        # 尾椎中心坐标
        x_center = self.count_rect_center(data['X'])
        cv.circle(self.img, x_center, 3, (0, 0, 255), -1)

        # 第一部分： 左臀上半圆坐标轨迹(左臀中心到尾椎中心为直径)
        step = 12
        x0 = lhip_center[0]
        x1 = x_center[0]
        per_distance = (x1 - x0) / step # 两坐标x之间距离分成8份，每份长度
        for i in range(1, step):
            x = x0 + per_distance * i
            _, y2 =self.solve_y_for_circle(lhip_center, x_center, x) # 只需上半圆，故只取y2
            cv.circle(self.img, (int(x), int(y2)), 3, (0, 0, 115), -1)
            print(f"x: {x}, y2: {y2}")

        print(f"lhip_center: {lhip_center}")
        print(f"x_center: {x_center}")
        print(f"per_distance: {per_distance}")

        # 第二部分： 右臀上半圆坐标轨迹(右臀中心到尾椎中心为直径)
        x0 = x_center[0]
        x1 = rhip_center[0]
        per_distance = (x1 - x0) / step # 两坐标x之间距离分成8份，每份长度
        for i in range(1, step):
            x = x0 + per_distance * i
            _, y2 =self.solve_y_for_circle(rhip_center, x_center, x) # 只需上半圆，故只取y2
            cv.circle(self.img, (int(x), int(y2)), 3, (0, 0, 115), -1)
            print(f"x: {x}, y2: {y2}")

        print(f"rhip_center: {lhip_center}")
        print(f"x_center: {x_center}")
        print(f"per_distance: {per_distance}")


if __name__ == "__main__":
    #data = {'neck': ['308.6964', '453.13934', '439.24008', '691.86646'], 'Lshoulder': ['489.00934', '249.40158', '873.007', '511.56537'], 'Rshoulder': ['463.79297', '614.3674', '873.6144', '915.96497'], 'Lback': ['855.87445', '335.03693', '1325.1007', '588.99475'], 'Rback': ['856.48596', '587.1396', '1338.5337', '859.5877'], 'Wback': ['846.78754', '294.97504', '1374.4795', '893.6671']}
    # 全背绘制运行坐标轨迹
    plan = PathPlan()
    plan.read_xy_data()
    data = plan.xy_data
    plan.draw_back('Wback', data['Wback'])
    cv.namedWindow("Image_2", 0)
    cv.imshow('Image_2', plan.img)
    
    # 其它部位绘制运行坐标轨迹
    plan = PathPlan()
    plan.draw_neck(data['neck'])
    plan.draw_shoulder('Lshoulder', data['Lshoulder'])
    plan.draw_shoulder('Rshoulder', data['Rshoulder'])
    plan.draw_back('Lback', data['Lback'])
    plan.draw_back('Rback', data['Rback'])
    plan.draw_hip_x(data)

    cv.namedWindow("Image", 0)
    cv.imshow('Image', plan.img)
    cv.waitKey(0)
    cv.destroyAllWindows()